/*
    机器人数字孪生
    Example sketch for TFT_eSPI library.
    No fonts are needed.
    Draws a 3d rotating cube on the TFT screen.
    Original code was found at http://forum.freetronics.com/viewtopic.php?f=37&t=5495
*/
#include "ocu.h"
VMC vmc[4];
VMC_ALL vmc_all;
robotTypeDef robotwb;

#define BLACK_C 0x0000
#define WHITE_C 0xFFFF

#include <SPI.h>
#include <TFT_eSPI.h> // Hardware-specific library

#define TERRAIN_NUM 36//地形最大网格数量
float terrain_range[2]={0.4,0.4};

int lock_yaw=0;//锁定显示角度
END_POS end_pos_stand_h[4], end_pos_stand_n[4];
float MIN_Z=-0.1;
float MAX_Z=-0.19;
float MIN_Y=-0.1;
float MAX_Y=-0.1;
float MIN_X=-0.15;
float MAX_X=0.15;
float MAX_SPD=0;

float MAX_SPD_X=0.4;
float MAX_SPD_Y=0.25;
float MAX_SPD_RAD=35;

int16_t h;
int16_t w;

int inc = -2;

float xx, xy, xz;
float yx, yy, yz;
float zx, zy, zz;

float fact;

int Xan, Yan;

int Xoff;
int Yoff;
int Zoff;

struct Point3d {
    float x;
    float y;
    float z;
};

struct Point2d {
    float x;
    float y;
};

int LinestoRender; // lines to render.
int OldLinestoRender; // lines to render just in case it changes. this makes sure the old lines all get erased.

struct Line3d {
    Point3d p0;
    Point3d p1;
};

struct Line2d {
    Point2d p0;
    Point2d p1;
};

#define LINE_R_QUAD 4+1+3*4+1+TERRAIN_NUM  //总线段显示数量

Line3d Lines[LINE_R_QUAD];
Line2d Render[LINE_R_QUAD];
Line2d ORender[LINE_R_QUAD];


Line3d Lines_quad[LINE_R_QUAD];
int    Lines_quad_c[LINE_R_QUAD];
Line2d Render_quad[LINE_R_QUAD];
Line2d ORender_quad[LINE_R_QUAD];


float sindw(float in){
    return sinf(in/57.3);
}

float cosdw(float in){
    return cosf(in/57.3);
}

float sind(float in){
    return sinf(in/57.3);
}

float cosd(float in){
    return cosf(in/57.3);
}


float To_180_degreesw(float x)
{
	return (x>180?(x-360):(x<-180?(x+360):x));
}

void mat_trans(float src[3][3], float dis[3][3])
{
    char i, j;
    float k;
    for (i = 0; i < 3; i++) {
        for (j = 0; j < 3; j++) {
            dis[i][j] = src[j][i];
        }
    }
}

//正运动学
char estimate_end_state_new(VMC *in, float dt)
{
    float H = in->l1 * cosdw(in->sita1) + in->l2 * cosdw(180 - in->sita2- in->sita1);
    in->epos.x = -in->l1 * sindw(in->sita1) + in->l2 * sindw(180 - in->sita2 - in->sita1);
    in->epos.x *= in->param.invert_knee_epos[Xr];

    in->epos.y = in->l3 * cosdw(in->sita3 * in->flag_rl)
        + H * sindw(in->sita3 * in->flag_rl);
    in->epos.y *= in->flag_rl;

    in->epos.z = -H * cosdw(in->sita3 * in->flag_rl)
        + in->l3 * sindw(in->sita3 * in->flag_rl);

    in->r = sqrtf(in->epos.x*in->epos.x
        + in->epos.y*in->epos.y
        + in->epos.z*in->epos.z);

    //END_POS hip_pos_b,kin1_pos_b,kin12_pos_b,kin2_pos_b;
    in->epos_b.x = in->epos.x + in->flag_fb* vmc_all.H / 2;
    in->epos_b.y = in->epos.y + in->flag_rl* vmc_all.W / 2;
    in->epos_b.z = in->epos.z;
    //足端
    in->kin2_pos_b=in->epos_b;

    in->kin2_pos_n.x = vmc_all.Rb_n[0][0] * in->kin2_pos_b.x + vmc_all.Rb_n[0][1] * in->kin2_pos_b.y + vmc_all.Rb_n[0][2] * in->kin2_pos_b.z;
    in->kin2_pos_n.y = vmc_all.Rb_n[1][0] * in->kin2_pos_b.x + vmc_all.Rb_n[1][1] * in->kin2_pos_b.y + vmc_all.Rb_n[1][2] * in->kin2_pos_b.z;
    in->kin2_pos_n.z = vmc_all.Rb_n[2][0] * in->kin2_pos_b.x + vmc_all.Rb_n[2][1] * in->kin2_pos_b.y + vmc_all.Rb_n[2][2] * in->kin2_pos_b.z;

    //跨关节
    in->hip_pos_b.x = in->flag_fb* vmc_all.H / 2;
    in->hip_pos_b.y = in->flag_rl* vmc_all.W / 2;
    in->hip_pos_b.z = 0;

    in->hip_pos_n.x = vmc_all.Rb_n[0][0] * in->hip_pos_b.x + vmc_all.Rb_n[0][1] * in->hip_pos_b.y + vmc_all.Rb_n[0][2] * in->hip_pos_b.z;
    in->hip_pos_n.y = vmc_all.Rb_n[1][0] * in->hip_pos_b.x + vmc_all.Rb_n[1][1] * in->hip_pos_b.y + vmc_all.Rb_n[1][2] * in->hip_pos_b.z;
    in->hip_pos_n.z = vmc_all.Rb_n[2][0] * in->hip_pos_b.x + vmc_all.Rb_n[2][1] * in->hip_pos_b.y + vmc_all.Rb_n[2][2] * in->hip_pos_b.z;
    //侧展电机1
    H = 0 * cosdw(in->sita1) + 0 * cosdw(180 - in->sita2- in->sita1);
    in->epos.x = -0 * sindw(in->sita1) + 0 * sindw(180 - in->sita2 - in->sita1);
    in->epos.x *= in->param.invert_knee_epos[Xr];

    in->epos.y = in->l3 * cosdw(in->sita3 * in->flag_rl)
        + H * sindw(in->sita3 * in->flag_rl);
    in->epos.y *= in->flag_rl;

    in->epos.z = -H * cosdw(in->sita3 * in->flag_rl)
        + in->l3 * sindw(in->sita3 * in->flag_rl);

    in->kin1_pos_b.x = in->epos.x + in->flag_fb* vmc_all.H / 2;
    in->kin1_pos_b.y = in->epos.y + in->flag_rl* vmc_all.W / 2;
    in->kin1_pos_b.z = in->epos.z;
    
    in->kin1_pos_n.x = vmc_all.Rb_n[0][0] * in->kin1_pos_b.x + vmc_all.Rb_n[0][1] * in->kin1_pos_b.y + vmc_all.Rb_n[0][2] * in->kin1_pos_b.z;
    in->kin1_pos_n.y = vmc_all.Rb_n[1][0] * in->kin1_pos_b.x + vmc_all.Rb_n[1][1] * in->kin1_pos_b.y + vmc_all.Rb_n[1][2] * in->kin1_pos_b.z;
    in->kin1_pos_n.z = vmc_all.Rb_n[2][0] * in->kin1_pos_b.x + vmc_all.Rb_n[2][1] * in->kin1_pos_b.y + vmc_all.Rb_n[2][2] * in->kin1_pos_b.z;
    //小腿电机位置
    H = in->l1 * cosdw(in->sita1) + 0 * cosdw(180 - in->sita2- in->sita1);
    in->epos.x = -in->l1 * sindw(in->sita1) + 0 * sindw(180 - in->sita2 - in->sita1);
    in->epos.x *= in->param.invert_knee_epos[Xr];

    in->epos.y = in->l3 * cosdw(in->sita3 * in->flag_rl)
        + H * sindw(in->sita3 * in->flag_rl);
    in->epos.y *= in->flag_rl;

    in->epos.z = -H * cosdw(in->sita3 * in->flag_rl)
        + in->l3 * sindw(in->sita3 * in->flag_rl);

    in->kin12_pos_b.x = in->epos.x + in->flag_fb* vmc_all.H / 2;
    in->kin12_pos_b.y = in->epos.y + in->flag_rl* vmc_all.W / 2;
    in->kin12_pos_b.z = in->epos.z;
   
    in->kin12_pos_n.x = vmc_all.Rb_n[0][0] * in->kin12_pos_b.x + vmc_all.Rb_n[0][1] * in->kin12_pos_b.y + vmc_all.Rb_n[0][2] * in->kin12_pos_b.z;
    in->kin12_pos_n.y = vmc_all.Rb_n[1][0] * in->kin12_pos_b.x + vmc_all.Rb_n[1][1] * in->kin12_pos_b.y + vmc_all.Rb_n[1][2] * in->kin12_pos_b.z;
    in->kin12_pos_n.z = vmc_all.Rb_n[2][0] * in->kin12_pos_b.x + vmc_all.Rb_n[2][1] * in->kin12_pos_b.y + vmc_all.Rb_n[2][2] * in->kin12_pos_b.z;
    //-----------------
    in->epos_n.x = vmc_all.Rb_n[0][0] * in->epos_b.x + vmc_all.Rb_n[0][1] * in->epos_b.y + vmc_all.Rb_n[0][2] * in->epos_b.z;
    in->epos_n.y = vmc_all.Rb_n[1][0] * in->epos_b.x + vmc_all.Rb_n[1][1] * in->epos_b.y + vmc_all.Rb_n[1][2] * in->epos_b.z;
    in->epos_n.z = vmc_all.Rb_n[2][0] * in->epos_b.x + vmc_all.Rb_n[2][1] * in->epos_b.y + vmc_all.Rb_n[2][2] * in->epos_b.z;

    in->epos_n_b.x = vmc_all.Rb_n[0][0] * in->epos.x + vmc_all.Rb_n[0][1] * in->epos.y + vmc_all.Rb_n[0][2] * in->epos.z;
    in->epos_n_b.y = vmc_all.Rb_n[1][0] * in->epos.x + vmc_all.Rb_n[1][1] * in->epos.y + vmc_all.Rb_n[1][2] * in->epos.z;
    in->epos_n_b.z = vmc_all.Rb_n[2][0] * in->epos.x + vmc_all.Rb_n[2][1] * in->epos.y + vmc_all.Rb_n[2][2] * in->epos.z;
   
    float size[2]={vmc_all.H/2*0.8,vmc_all.W/2*0.8};
    vmc_all.tar_spd_v.x=ocu.rc_spd_w[0]*size[0];
    vmc_all.tar_spd_v.y=ocu.rc_spd_w[1]*size[1];
    vmc_all.tar_spd_v.z=0;

    
    vmc_all.tar_spd_vn.x = vmc_all.Rb_n_noatt[0][0] *  vmc_all.tar_spd_v.x + vmc_all.Rb_n_noatt[0][1] *  vmc_all.tar_spd_v.y + vmc_all.Rb_n_noatt[0][2] *  vmc_all.tar_spd_v.z;
    vmc_all.tar_spd_vn.y = vmc_all.Rb_n_noatt[1][0] *  vmc_all.tar_spd_v.x + vmc_all.Rb_n_noatt[1][1] *  vmc_all.tar_spd_v.y + vmc_all.Rb_n_noatt[1][2] *  vmc_all.tar_spd_v.z;
    vmc_all.tar_spd_vn.z = vmc_all.Rb_n_noatt[2][0] *  vmc_all.tar_spd_v.x + vmc_all.Rb_n_noatt[2][1] *  vmc_all.tar_spd_v.y + vmc_all.Rb_n_noatt[2][2] *  vmc_all.tar_spd_v.z;
    return 0;
}

void quad_robot(float dt) {//机器人坐标系下线段起始点
    int line_id=0;
    #if 0//-------------------------机体坐标系------------------（目前不使用）
        line_id=0;
        Lines[line_id].p0.x = vmc[0].hip_pos_b.x;
        Lines[line_id].p0.y = vmc[0].hip_pos_b.y;
        Lines[line_id].p0.z = vmc[0].hip_pos_b.z;
        Lines[line_id].p1.x = vmc[1].hip_pos_b.x;
        Lines[line_id].p1.y = vmc[1].hip_pos_b.y;
        Lines[line_id].p1.z = vmc[1].hip_pos_b.z;
        line_id++;
        Lines[line_id].p0.x = vmc[1].hip_pos_b.x;
        Lines[line_id].p0.y = vmc[1].hip_pos_b.y;
        Lines[line_id].p0.z = vmc[1].hip_pos_b.z;
        Lines[line_id].p1.x = vmc[3].hip_pos_b.x;
        Lines[line_id].p1.y = vmc[3].hip_pos_b.y;
        Lines[line_id].p1.z = vmc[3].hip_pos_b.z;
        line_id++;
        Lines[line_id].p0.x = vmc[3].hip_pos_b.x;
        Lines[line_id].p0.y = vmc[3].hip_pos_b.y;
        Lines[line_id].p0.z = vmc[3].hip_pos_b.z;
        Lines[line_id].p1.x = vmc[2].hip_pos_b.x;
        Lines[line_id].p1.y = vmc[2].hip_pos_b.y;
        Lines[line_id].p1.z = vmc[2].hip_pos_b.z;
        line_id++;
        Lines[line_id].p0.x = vmc[2].hip_pos_b.x;
        Lines[line_id].p0.y = vmc[2].hip_pos_b.y;
        Lines[line_id].p0.z = vmc[2].hip_pos_b.z;
        Lines[line_id].p1.x = vmc[0].hip_pos_b.x;
        Lines[line_id].p1.y = vmc[0].hip_pos_b.y;
        Lines[line_id].p1.z = vmc[0].hip_pos_b.z;

    //-------------------FR----------------------
        for(int i=0;i<4;i++){
      //END_POS hip_pos_b,kin1_pos_b,kin12_pos_b,kin2_pos_b;
        line_id++;
        Lines[line_id].p0.x = vmc[i].hip_pos_b.x;
        Lines[line_id].p0.y = vmc[i].hip_pos_b.y;
        Lines[line_id].p0.z = vmc[i].hip_pos_b.z;
        Lines[line_id].p1.x = vmc[i].kin1_pos_b.x;
        Lines[line_id].p1.y = vmc[i].kin1_pos_b.y;
        Lines[line_id].p1.z = vmc[i].kin1_pos_b.z;
        line_id++;
        Lines[line_id].p0.x = vmc[i].kin1_pos_b.x;
        Lines[line_id].p0.y = vmc[i].kin1_pos_b.y;
        Lines[line_id].p0.z = vmc[i].kin1_pos_b.z;
        Lines[line_id].p1.x = vmc[i].kin12_pos_b.x;
        Lines[line_id].p1.y = vmc[i].kin12_pos_b.y;
        Lines[line_id].p1.z = vmc[i].kin12_pos_b.z;
        line_id++;
        Lines[line_id].p0.x = vmc[i].kin12_pos_b.x;
        Lines[line_id].p0.y = vmc[i].kin12_pos_b.y;
        Lines[line_id].p0.z = vmc[i].kin12_pos_b.z;
        Lines[line_id].p1.x = vmc[i].kin2_pos_b.x;
        Lines[line_id].p1.y = vmc[i].kin2_pos_b.y;
        Lines[line_id].p1.z = vmc[i].kin2_pos_b.z;
        }
    #else//-------------------------------全局坐标系----------------------------------
        //渲染机体
        Lines[line_id].p0.x = vmc[0].hip_pos_n.x;
        Lines[line_id].p0.y = vmc[0].hip_pos_n.y;
        Lines[line_id].p0.z = vmc[0].hip_pos_n.z;
        Lines[line_id].p1.x = vmc[1].hip_pos_n.x;
        Lines[line_id].p1.y = vmc[1].hip_pos_n.y;
        Lines[line_id].p1.z = vmc[1].hip_pos_n.z;
        Lines_quad_c[line_id++]=TFT_BLUE;
  
        Lines[line_id].p0.x = vmc[1].hip_pos_n.x;
        Lines[line_id].p0.y = vmc[1].hip_pos_n.y;
        Lines[line_id].p0.z = vmc[1].hip_pos_n.z;
        Lines[line_id].p1.x = vmc[3].hip_pos_n.x;
        Lines[line_id].p1.y = vmc[3].hip_pos_n.y;
        Lines[line_id].p1.z = vmc[3].hip_pos_n.z;
        Lines_quad_c[line_id++]=TFT_BLUE;

        Lines[line_id].p0.x = vmc[3].hip_pos_n.x;
        Lines[line_id].p0.y = vmc[3].hip_pos_n.y;
        Lines[line_id].p0.z = vmc[3].hip_pos_n.z;
        Lines[line_id].p1.x = vmc[2].hip_pos_n.x;
        Lines[line_id].p1.y = vmc[2].hip_pos_n.y;
        Lines[line_id].p1.z = vmc[2].hip_pos_n.z;
        Lines_quad_c[line_id++]=TFT_BLUE;
     
        Lines[line_id].p0.x = vmc[2].hip_pos_n.x;
        Lines[line_id].p0.y = vmc[2].hip_pos_n.y;
        Lines[line_id].p0.z = vmc[2].hip_pos_n.z;
        Lines[line_id].p1.x = vmc[0].hip_pos_n.x;
        Lines[line_id].p1.y = vmc[0].hip_pos_n.y;
        Lines[line_id].p1.z = vmc[0].hip_pos_n.z;
        Lines_quad_c[line_id++]=TFT_YELLOW;

    //-------------------渲染腿-------------------
        for(int i=0;i<4;i++){
          Lines[line_id].p0.x = vmc[i].hip_pos_n.x;
          Lines[line_id].p0.y = vmc[i].hip_pos_n.y;
          Lines[line_id].p0.z = vmc[i].hip_pos_n.z;
          Lines[line_id].p1.x = vmc[i].kin1_pos_n.x;
          Lines[line_id].p1.y = vmc[i].kin1_pos_n.y;
          Lines[line_id].p1.z = vmc[i].kin1_pos_n.z;
          Lines_quad_c[line_id++]=TFT_RED;
    
          Lines[line_id].p0.x = vmc[i].kin1_pos_n.x;
          Lines[line_id].p0.y = vmc[i].kin1_pos_n.y;
          Lines[line_id].p0.z = vmc[i].kin1_pos_n.z;
          Lines[line_id].p1.x = vmc[i].kin12_pos_n.x;
          Lines[line_id].p1.y = vmc[i].kin12_pos_n.y;
          Lines[line_id].p1.z = vmc[i].kin12_pos_n.z;
          Lines_quad_c[line_id++]=TFT_WHITE;
        
          Lines[line_id].p0.x = vmc[i].kin12_pos_n.x;
          Lines[line_id].p0.y = vmc[i].kin12_pos_n.y;
          Lines[line_id].p0.z = vmc[i].kin12_pos_n.z;
          Lines[line_id].p1.x = vmc[i].kin2_pos_n.x;
          Lines[line_id].p1.y = vmc[i].kin2_pos_n.y;
          Lines[line_id].p1.z = vmc[i].kin2_pos_n.z;
          Lines_quad_c[line_id++]=TFT_GREEN;
        }

        //----------------渲染期望质心速度指令
        Lines[line_id].p0.x =  0;
        Lines[line_id].p0.y =  0;
        Lines[line_id].p0.z =  0;
        Lines[line_id].p1.x =  vmc_all.tar_spd_vn.x;
        Lines[line_id].p1.y =  vmc_all.tar_spd_vn.y;
        Lines[line_id].p1.z =  vmc_all.tar_spd_vn.z;
        Lines_quad_c[line_id++]=TFT_GREEN;
        //----------------------渲染地形-------------------------------
 
        for(int i=0;i<TERRAIN_NUM;i++){
          //移动地形标志
          // vmc_all.spd_n.x=0.05;
          // vmc_all.spd_n.y=0.05;
          vmc_all.terrain_map[i][0].x+=-vmc_all.spd_n.x*dt;
          vmc_all.terrain_map[i][0].y+=-vmc_all.spd_n.y*dt;
          vmc_all.terrain_map[i][1].x=vmc_all.terrain_map[i][0].x;
          vmc_all.terrain_map[i][1].y=vmc_all.terrain_map[i][0].y;
      #if 1
          if(vmc_all.terrain_map[i][0].x>terrain_range[0]+0.05*0)//超出显示复位X
          {
            vmc_all.terrain_map[i][0].x+=-2*terrain_range[0];
            vmc_all.terrain_map[i][1].x+=-2*terrain_range[0];
          }else if(vmc_all.terrain_map[i][0].x<-terrain_range[0]-0.05*0)//超出显示复位X
          {
            vmc_all.terrain_map[i][0].x+=2*terrain_range[0];
            vmc_all.terrain_map[i][1].x+=2*terrain_range[0];
          }

          if(vmc_all.terrain_map[i][0].y>terrain_range[1]+0.05*0)//超出显示复位Y
          {
            vmc_all.terrain_map[i][0].y+=-2*terrain_range[1];
            vmc_all.terrain_map[i][1].y+=-2*terrain_range[1];
          }else if(vmc_all.terrain_map[i][0].y<-terrain_range[1]-0.05*0)//超出显示复位Y
          {
            vmc_all.terrain_map[i][0].y+=2*terrain_range[1];
            vmc_all.terrain_map[i][1].y+=2*terrain_range[1];
          }
    #endif
          Lines[line_id].p0.x =  vmc_all.terrain_map[i][0].x;
          Lines[line_id].p0.y =  vmc_all.terrain_map[i][0].y;
          Lines[line_id].p0.z =  vmc_all.terrain_map[i][0].z-nav_rx.cog_pos_n.z;
          Lines[line_id].p1.x =  vmc_all.terrain_map[i][1].x;
          Lines[line_id].p1.y =  vmc_all.terrain_map[i][1].y;
          Lines[line_id].p1.z =  vmc_all.terrain_map[i][1].z-nav_rx.cog_pos_n.z;
          Lines_quad_c[line_id++]=TFT_DARKGREY;
          //Lines_quad_c[line_id++]=TFT_WHITE;
        }
 #endif//end 全局选择

    
    int size=230;//显示尺寸缩放
    for(int i=0;i<LINE_R_QUAD;i++)
    {
      Lines[i].p0.x *=size;
      Lines[i].p0.y *=size;
      Lines[i].p0.z *=size;
      Lines[i].p1.x *=size;
      Lines[i].p1.y *=size;
      Lines[i].p1.z *=size;
    }

    LinestoRender = LINE_R_QUAD;//最大线数量
    OldLinestoRender = LinestoRender;
}

/***********************************************************************************************************************************/
void robot_setup(int sel) {//初始化机器人参数
    //lock_yaw=sel;
    h = tft.height();
    w = tft.width();

    vmc_all.H=0.5;//机体长度
    vmc_all.W=0.204;
    #if 1//默认初始化角度
    vmc[0].sita1=45;vmc[0].sita2=90; vmc[0].sita3=-15;
    vmc[1].sita1=45;vmc[1].sita2=90; vmc[1].sita3=-15;
    vmc[2].sita1=45;vmc[2].sita2=90; vmc[2].sita3=15;
    vmc[3].sita1=45;vmc[3].sita2=90; vmc[3].sita3=15;
    #endif
    
    vmc[FL1].flag_rl=vmc[BL1].flag_rl=1;
    vmc[FL2].flag_rl=vmc[BL2].flag_rl=-1;
    vmc[FL1].flag_fb=vmc[FL2].flag_fb=1;
    vmc[BL1].flag_fb=vmc[BL2].flag_fb=-1;
    //各腿正反肘标志位 1反肘 -1正肘 腿号FR:0 HR:1 FL:2 HL:3
    vmc[0].param.invert_knee_epos[Xr]=1;
    vmc[1].param.invert_knee_epos[Xr]=-1;
    vmc[2].param.invert_knee_epos[Xr]=1;
    vmc[3].param.invert_knee_epos[Xr]=-1;
            
    vmc_all.MAX_PIT=10;
    vmc_all.MAX_ROL=15;
    vmc_all.MAX_YAW=35;

    float att_rt_use[3]={0,0,0};
    att_rt_use[0]=nav_rx.att_now[0];
    att_rt_use[1]=nav_rx.att_now[1];
    
    vmc_all.Rn_b[0][0] = cosd(-att_rt_use[PITr])*cosd(-att_rt_use[YAWr]);
    vmc_all.Rn_b[1][0] = -cosd(-att_rt_use[ROLr])*sind(-att_rt_use[YAWr]) + sind(-att_rt_use[PITr])*sind(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]);
    vmc_all.Rn_b[2][0] = sind(-att_rt_use[ROLr])*sind(-att_rt_use[YAWr]) + cosd(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*cosd(-att_rt_use[YAWr]);

    vmc_all.Rn_b[0][1] = cosd(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);
    vmc_all.Rn_b[1][1] = cosd(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]) + sind(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);
    vmc_all.Rn_b[2][1] = -sind(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]) + cosd(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);

    vmc_all.Rn_b[0][2] = -sind(-att_rt_use[PITr]);
    vmc_all.Rn_b[1][2] = sind(-att_rt_use[ROLr])*cosd(-att_rt_use[PITr]);
    vmc_all.Rn_b[2][2] = cosd(-att_rt_use[ROLr])*cosd(-att_rt_use[PITr]);
    mat_trans(vmc_all.Rn_b, vmc_all.Rb_n);

    att_rt_use[0]=nav_rx.att_now[0]*0;
    att_rt_use[1]=nav_rx.att_now[1]*0;
    if(lock_yaw)
      att_rt_use[2]=0;
    else
      att_rt_use[2]=nav_rx.att_now[2]*-1;

    vmc_all.Rn_b_noatt[0][0] = cosd(-att_rt_use[PITr])*cosd(-att_rt_use[YAWr]);
    vmc_all.Rn_b_noatt[1][0] = -cosd(-att_rt_use[ROLr])*sind(-att_rt_use[YAWr]) + sind(-att_rt_use[PITr])*sind(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]);
    vmc_all.Rn_b_noatt[2][0] = sind(-att_rt_use[ROLr])*sind(-att_rt_use[YAWr]) + cosd(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*cosd(-att_rt_use[YAWr]);

    vmc_all.Rn_b_noatt[0][1] = cosd(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);
    vmc_all.Rn_b_noatt[1][1] = cosd(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]) + sind(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);
    vmc_all.Rn_b_noatt[2][1] = -sind(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]) + cosd(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);

    vmc_all.Rn_b_noatt[0][2] = -sind(-att_rt_use[PITr]);
    vmc_all.Rn_b_noatt[1][2] = sind(-att_rt_use[ROLr])*cosd(-att_rt_use[PITr]);
    vmc_all.Rn_b_noatt[2][2] = cosd(-att_rt_use[ROLr])*cosd(-att_rt_use[PITr]);
    mat_trans(vmc_all.Rn_b_noatt, vmc_all.Rb_n_noatt);

    for(int i=0;i<4;i++)
    {
      vmc_all.l1=vmc[i].l1=0.22;//大腿长度
      vmc_all.l2=vmc[i].l2=0.24;//小腿长度
      vmc_all.l3=vmc[i].l3=0.111;//侧展偏距
      estimate_end_state_new(&vmc[i],0);
      end_pos_stand_h[i] = vmc[i].epos;
      end_pos_stand_n[i] = vmc[i].epos_n;
//      Serial.print(i);
//      Serial.print("|");
//      Serial.print(end_pos_stand_h[i].x);
//      Serial.print(" ");
//      Serial.print(end_pos_stand_h[i].y);
//      Serial.print(" ");
//      Serial.println(end_pos_stand_h[i].z);
    }

    MIN_Z = -sind(10)*vmc_all.l1 - sind(20)*vmc_all.l2;//0.04;
    MAX_Z = -sind(75)*vmc_all.l1 - sind(75)*vmc_all.l2;//0.09;

    MIN_Y = -sind(55)*(vmc_all.l1 + vmc_all.l2)*0.8;//0.06;
    MAX_Y = sind(55)*(vmc_all.l1 + vmc_all.l2)*0.8;
    MIN_X = -sind(65)*vmc_all.l2;
    MAX_X = sind(65)*vmc_all.l2;

    vmc_all.test_pos_flt[Zr] = -0.8*MAX_Z;
    nav_rx.cog_pos_n.z=vmc_all.test_pos_flt[Zr];
    vmc_all.test_pos_flt[Xr] = vmc_all.test_pos_flt[Yr] = 0;
    nav_rx.cog_pos_n.x=nav_rx.cog_pos_n.y=0;
    //初始化相机参数
    if(sel==1){//上帝界面
      terrain_range[0]=1.2;//地图尺寸
      terrain_range[1]=1.2;
      fact = 110 / 3.14159259; // conversion from degrees to radians. ROL角度 小角度看到肚子 大角度俯视

      Xoff = 160;//210; // Position the center of the 3d conversion space into the center of the TFT screen.
      Yoff = 120;//70;//大了往下
      Zoff = 340; // Z offset in 3D space (smaller = closer and bigger rendering) 远近
    }else{//系统界面
      terrain_range[0]=0.5;
      terrain_range[1]=0.5;
      fact = 100 / 3.14159259; // conversion from degrees to radians. ROL角度 小角度看到肚子 大角度俯视

      Xoff = 240;//210; // Position the center of the 3d conversion space into the center of the TFT screen.
      Yoff = 80;//70;//大了往下
      Zoff = 400; // Z offset in 3D space (smaller = closer and bigger rendering) 远近
    }

    //初始化地形
    float terrain_h=0.015;//地形线段高度
    int terrain_id=0;
    for(int i=0;i<(int)sqrt(TERRAIN_NUM);i++){
      for(int j=0;j<(int)sqrt(TERRAIN_NUM);j++){
      vmc_all.terrain_map[i*(int)sqrt(TERRAIN_NUM)+j][0].x=  vmc_all.terrain_map[i*(int)sqrt(TERRAIN_NUM)+j][1].x=-terrain_range[0]+i*2*terrain_range[0]/(sqrt(TERRAIN_NUM));
      vmc_all.terrain_map[i*(int)sqrt(TERRAIN_NUM)+j][0].y=  vmc_all.terrain_map[i*(int)sqrt(TERRAIN_NUM)+j][1].y=-terrain_range[1]+j*2*terrain_range[1]/(sqrt(TERRAIN_NUM));
      vmc_all.terrain_map[i*(int)sqrt(TERRAIN_NUM)+j][0].z=0;vmc_all.terrain_map[i*(int)sqrt(TERRAIN_NUM)+j][1].z=terrain_h;
      }
    }
}


void inv_KI(int id, Vect3 epos_h, float *s0, float *s1,float *s2)
{
    float end_pos[3];
    float temp[6];
    float L1w=vmc_all.l1;
    float L2w=vmc_all.l2;
    float L3w=vmc_all.l3;
    float x = epos_h.x;
    float z = epos_h.z;
    end_pos[Xr] = vmc[id].param.invert_knee_epos[Xr]*epos_h.x;
    end_pos[Yr] = vmc[id].flag_rl* epos_h.y;
    end_pos[Zr] = epos_h.z;
    float Rr = end_pos[Xr] * end_pos[Xr] + end_pos[Yr] * end_pos[Yr] + end_pos[Zr] * end_pos[Zr];
    float Rr1 = end_pos[Yr] * end_pos[Yr] + end_pos[Zr] * end_pos[Zr];
    temp[2] = sqrtf(LIMIT(-L3w * L3w + Rr1, 0, 99));
    *s2 = (90 - atan2(end_pos[Zr], end_pos[Yr] + 0.000001)*RAD_TO_DEGw -
        atan2(L3w, temp[2] + 0.000001)*RAD_TO_DEGw)*vmc[id].flag_rl;
    temp[0] = sqrtf(LIMIT(Rr1 - L3w * L3w, 0, 99));//h1
    temp[1] = sqrtf(LIMIT(temp[0] * temp[0] + end_pos[Xr] * end_pos[Xr], 0, 99));//r
    temp[2] = atan2(end_pos[Xr], temp[0] + 0.000001)*RAD_TO_DEGw;//sita
    temp[3] = (temp[1] * temp[1] + L1w * L1w - L2w * L2w) / (2 * L1w*temp[1] + 0.000001);
    temp[3] = LIMIT(temp[3], -1, 1);
    *s0 = 180 - (180 + temp[2] - acos(temp[3]) *RAD_TO_DEGw);

    temp[0] = (L1w*L1w + L2w * L2w + L3w * L3w - Rr) / (2 * L1w*L2w);
    temp[0] = LIMIT(temp[0], -1, 1);
    *s1 = acos(temp[0])*RAD_TO_DEGw;
}

char inv_end_state_new(VMC *vmc, float x, float y, float z, float *sita1, float *sita2, float *sita3)
{
    int i = vmc->param.id;
    robotwb.Leg[i].tar_epos_h.x = x;
    robotwb.Leg[i].tar_epos_h.y = y;
    robotwb.Leg[i].tar_epos_h.z = z;
    inv_KI(i, robotwb.Leg[i].tar_epos_h, sita1, sita2, sita3);
//    Serial.print(i);
//    Serial.print("|");
//    Serial.print(x);
//    Serial.print(" ");
//    Serial.print(y);
//    Serial.print(" ");
//    Serial.print(z);
//    Serial.print("|");
//    Serial.print(*sita1);
//    Serial.print(" ");
//    Serial.print(*sita2);
//    Serial.print(" ");
//    Serial.println(*sita3);
    return 1;
}

void stand_ik(float dt){
    int i=0;
    static END_POS end_pos_n[4];
    static float att_test_use[3];
    int sit_down_flag=1;
    
    for (i = 0; i < 4; i++) {//期望末端位置
        end_pos_n[i].x = end_pos_stand_n[i].x;
        end_pos_n[i].y = end_pos_stand_n[i].y;
        end_pos_n[i].z = end_pos_stand_n[i].z;
//        Serial.print(i);
//        Serial.print("|");
//        Serial.print(end_pos_n[i].x);
//        Serial.print(" ");
//        Serial.print(end_pos_n[i].y);
//        Serial.print(" ");
//        Serial.println(end_pos_n[i].z);
    }

    DigitalLPF(-ocu.rc_att_w[PITr] * vmc_all.MAX_PIT, &vmc_all.test_att_flt[PITr], 0.85, dt);
    DigitalLPF(-ocu.rc_att_w[ROLr] * vmc_all.MAX_ROL, &vmc_all.test_att_flt[ROLr], 0.85, dt);
    DigitalLPF(ocu.rate_yaw_w*vmc_all.MAX_YAW, &vmc_all.test_att_flt[YAWr], 0.85, dt);

    //位置
//    vmc_all.test_pos_flt[Xr] += ocu.rc_spd_w[Xr] * 0.06*dt;
//    vmc_all.test_pos_flt[Xr] = LIMIT(vmc_all.test_pos_flt[Xr], 0.35*MIN_X, 0.35*MAX_X);
    vmc_all.test_pos_flt[Xr] = sind(vmc_all.test_att_flt[PITr])* vmc_all.H/2;
    
//    vmc_all.test_pos_flt[Yr] += ocu.rc_spd_w[Yr] * 0.06*dt;
//    vmc_all.test_pos_flt[Yr] = LIMIT(vmc_all.test_pos_flt[Yr], 0.5*MIN_Y, 0.5*MAX_Y);
    vmc_all.test_pos_flt[Yr] = sind(vmc_all.test_att_flt[ROLr])* vmc_all.W/2;
    
    if (sit_down_flag == 0) {
        DigitalLPF(LIMIT(vmc_all.test_att_flt[ROLr]  , -66, 66), &att_test_use[ROLr], 10, dt);//
        DigitalLPF(LIMIT(-vmc_all.test_att_flt[PITr] , -30, 30), &att_test_use[PITr], 10, dt);//
    }
    else {
        DigitalLPF(LIMIT(-vmc_all.test_att_flt[ROLr] , -66, 66), &att_test_use[ROLr], 10, dt);//
        DigitalLPF(LIMIT(-vmc_all.test_att_flt[PITr] , -66, 66), &att_test_use[PITr], 10, dt);//
    }
    DigitalLPF(LIMIT(vmc_all.test_att_flt[YAWr], -45, 45) , &att_test_use[YAWr], 10, dt);

    nav_rx.att_now[0]=-att_test_use[PITr];
    nav_rx.att_now[1]=-att_test_use[ROLr];
    nav_rx.att_now[2]= att_test_use[YAWr];
//    Serial.print(att_test_use[PITr]);
//    Serial.print(" ");
//    Serial.print(att_test_use[ROLr]);
//    Serial.print(" ");
//    Serial.println(att_test_use[YAWr]);

    float RTb_n_target[3][3];//
    RTb_n_target[0][0] = cosd(-att_test_use[PITr]);  RTb_n_target[0][1] = sind(-att_test_use[PITr])*sind(-att_test_use[ROLr]); RTb_n_target[0][2] = sind(-att_test_use[PITr])*cosd(-att_test_use[ROLr]);
    RTb_n_target[1][0] = 0;                          RTb_n_target[1][1] = cosd(-att_test_use[ROLr]);                            RTb_n_target[1][2] = -sind(-att_test_use[ROLr]);
    RTb_n_target[2][0] = -sind(-att_test_use[PITr]); RTb_n_target[2][1] = cosd(-att_test_use[PITr])*sind(-att_test_use[ROLr]); RTb_n_target[2][2] = cosd(-att_test_use[PITr])*cosd(-att_test_use[ROLr]);

    END_POS angke_n[4];//
    for (i = 0; i < 4; i++)
        converV_b_to_n_RT(RTb_n_target, att_test_use[YAWr], vmc[i].flag_fb*vmc_all.H / 2, vmc[i].flag_rl*vmc_all.W / 2, 0, &angke_n[i].x, &angke_n[i].y, &angke_n[i].z);
    //----
    END_POS leg_2_angke_n[4];//
    for (i = 0; i < 4; i++) {
        leg_2_angke_n[i].x = end_pos_n[i].x - vmc[i].flag_fb*vmc_all.test_pos_flt[Xr] - angke_n[i].x ;
        leg_2_angke_n[i].y = end_pos_n[i].y - vmc[i].flag_rl*vmc_all.test_pos_flt[Yr] - angke_n[i].y ;
        leg_2_angke_n[i].z = vmc_all.test_pos_flt[Zr] - angke_n[i].z;
//        Serial.print(i);
//        Serial.print("|");
//        Serial.print(leg_2_angke_n[i].x);
//        Serial.print(" ");
//        Serial.print(leg_2_angke_n[i].y);
//        Serial.print(" ");
//        Serial.print(leg_2_angke_n[i].z);
//        Serial.print("|");
//        Serial.print(vmc_all.test_pos_flt[Zr]);
//        Serial.print(" ");
//        Serial.println(angke_n[i].z);
    }
    //----
    END_POS leg_2_angke_b[4];
    for (i = 0; i < 4; i++) {
        converV_n_to_b_w_yaw(-att_test_use[YAWr], leg_2_angke_n[i].x, leg_2_angke_n[i].y, leg_2_angke_n[i].z,
            &leg_2_angke_b[i].x, &leg_2_angke_b[i].y, &leg_2_angke_b[i].z);
        leg_2_angke_b[i].x += vmc[i].flag_fb*vmc_all.H / 2;
        leg_2_angke_b[i].y += vmc[i].flag_rl*vmc_all.W / 2;
    }
    //----
    END_POS leg_2_angke_leg[4];
    for (i = 0; i < 4; i++)
        converV_b_to_leg(i, leg_2_angke_b[i].x, leg_2_angke_b[i].y, leg_2_angke_b[i].z,
            &leg_2_angke_leg[i].x, &leg_2_angke_leg[i].y, &leg_2_angke_leg[i].z);
            
    for (i = 0; i < 4; i++)
        inv_end_state_new(&vmc[i], leg_2_angke_leg[i].x, leg_2_angke_leg[i].y, leg_2_angke_leg[i].z,
            &vmc[i].tar_sita1, &vmc[i].tar_sita2, &vmc[i].tar_sita3);
}
 
void update_robot(float dt)//刷新与渲染
{
    static float yaw_test=0;
    //读取关节角度
    if(robot_connect){
      vmc[0].sita1=nav_rx.sita_now[0][0];vmc[0].sita2=nav_rx.sita_now[0][1];vmc[0].sita3=nav_rx.sita_now[0][2];
      vmc[1].sita1=nav_rx.sita_now[1][0];vmc[1].sita2=nav_rx.sita_now[1][1];vmc[1].sita3=nav_rx.sita_now[1][2];
      vmc[2].sita1=nav_rx.sita_now[2][0];vmc[2].sita2=nav_rx.sita_now[2][1];vmc[2].sita3=nav_rx.sita_now[2][2];
      vmc[3].sita1=nav_rx.sita_now[3][0];vmc[3].sita2=nav_rx.sita_now[3][1];vmc[3].sita3=nav_rx.sita_now[3][2];
    }else if(0){//未连接机器人  简单步态
      if(connect_io){//连接IO板子
        ocu.rc_att_w[0]=rc[0];
        ocu.rc_att_w[1]=rc[1];
      }
      if(!wifi_search)
        stand_ik(dt);
        
      vmc[0].sita1=vmc[0].tar_sita1;vmc[0].sita2=vmc[0].tar_sita2;vmc[0].sita3=vmc[0].tar_sita3;
      vmc[1].sita1=vmc[1].tar_sita1;vmc[1].sita2=vmc[1].tar_sita2;vmc[1].sita3=vmc[1].tar_sita3;
      vmc[2].sita1=vmc[2].tar_sita1;vmc[2].sita2=vmc[2].tar_sita2;vmc[2].sita3=vmc[2].tar_sita3;
      vmc[3].sita1=vmc[3].tar_sita1;vmc[3].sita2=vmc[3].tar_sita2;vmc[3].sita3=vmc[3].tar_sita3;
    }
    
    for(int i=0;i<4;i++)//运动学正解
    {
      estimate_end_state_new(&vmc[i],0);
    }

    float att_rt_use[3]={0,0,0};
    att_rt_use[0]=nav_rx.att_now[0];
    att_rt_use[1]=nav_rx.att_now[1];
    
    yaw_test=90;//固定显示角度
    if(lock_yaw)
      att_rt_use[2]=yaw_test;
    else
      att_rt_use[2]=nav_rx.att_now[2]*-1;//全局显示

    vmc_all.Rn_b[0][0] =  cosd(-att_rt_use[PITr])*cosd(-att_rt_use[YAWr]);
    vmc_all.Rn_b[1][0] = -cosd(-att_rt_use[ROLr])*sind(-att_rt_use[YAWr]) + sind(-att_rt_use[PITr])*sind(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]);
    vmc_all.Rn_b[2][0] =  sind(-att_rt_use[ROLr])*sind(-att_rt_use[YAWr]) + cosd(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*cosd(-att_rt_use[YAWr]);

    vmc_all.Rn_b[0][1] =  cosd(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);
    vmc_all.Rn_b[1][1] =  cosd(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]) + sind(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);
    vmc_all.Rn_b[2][1] = -sind(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]) + cosd(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);

    vmc_all.Rn_b[0][2] = -sind(-att_rt_use[PITr]);
    vmc_all.Rn_b[1][2] =  sind(-att_rt_use[ROLr])*cosd(-att_rt_use[PITr]);
    vmc_all.Rn_b[2][2] =  cosd(-att_rt_use[ROLr])*cosd(-att_rt_use[PITr]);
    mat_trans(vmc_all.Rn_b, vmc_all.Rb_n);

    att_rt_use[0]=nav_rx.att_now[0]*0;
    att_rt_use[1]=nav_rx.att_now[1]*0;
    if(lock_yaw)
      att_rt_use[2]=0;
    else
      att_rt_use[2]=nav_rx.att_now[2]*-1;

    vmc_all.Rn_b_noatt[0][0] = cosd(-att_rt_use[PITr])*cosd(-att_rt_use[YAWr]);
    vmc_all.Rn_b_noatt[1][0] = -cosd(-att_rt_use[ROLr])*sind(-att_rt_use[YAWr]) + sind(-att_rt_use[PITr])*sind(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]);
    vmc_all.Rn_b_noatt[2][0] = sind(-att_rt_use[ROLr])*sind(-att_rt_use[YAWr]) + cosd(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*cosd(-att_rt_use[YAWr]);

    vmc_all.Rn_b_noatt[0][1] = cosd(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);
    vmc_all.Rn_b_noatt[1][1] = cosd(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]) + sind(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);
    vmc_all.Rn_b_noatt[2][1] = -sind(-att_rt_use[ROLr])*cosd(-att_rt_use[YAWr]) + cosd(-att_rt_use[ROLr])*sind(-att_rt_use[PITr])*sind(-att_rt_use[YAWr]);

    vmc_all.Rn_b_noatt[0][2] = -sind(-att_rt_use[PITr]);
    vmc_all.Rn_b_noatt[1][2] = sind(-att_rt_use[ROLr])*cosd(-att_rt_use[PITr]);
    vmc_all.Rn_b_noatt[2][2] = cosd(-att_rt_use[ROLr])*cosd(-att_rt_use[PITr]);
    mat_trans(vmc_all.Rn_b_noatt, vmc_all.Rb_n_noatt);

    converV_n_to_b_w_yaw(nav_rx.att_now[2],nav_rx.cog_spd_n.x,nav_rx.cog_spd_n.y,0,&vmc_all.spd_n.x,&vmc_all.spd_n.y,&vmc_all.spd_n.z);

    quad_robot(dt);
}
/***********************************************************************************************************************************/
void robot_loop(int pit,int rol,int yaw,float dt) {

    float flt=0.2;
    Xan = (pit % 360)*flt+(1-flt)*Xan;
    Yan = (rol % 360)*flt+(1-flt)*Yan; // prevents overflow.

    if(robot_connect||1)
      update_robot(dt);

    SetVars(); //sets up the global vars to do the 3D conversion.

    for (int i = 0; i < LinestoRender ; i++) {
        ORender[i] = Render[i]; // stores the old line segment so we can delete it later.
        ProcessLine(&Render[i], Lines[i]); // converts the 3d line segments to 2d.
    }
    RenderImage(); // go draw it!
}

/***********************************************************************************************************************************/
void RenderImage(void) {
    // renders all the lines after erasing the old ones.
    // in here is the only code actually interfacing with the OLED. so if you use a different lib, this is where to change it.

    for (int i = 0; i < OldLinestoRender; i++) {
        tft.drawLine(ORender[i].p0.x, ORender[i].p0.y, ORender[i].p1.x, ORender[i].p1.y, BLACK_C); // erase the old lines.
    }

    for (int i = 0; i < LinestoRender; i++) {
        uint16_t color = TFT_BLUE;
        color=Lines_quad_c[i];
        tft.drawLine(Render[i].p0.x, Render[i].p0.y, Render[i].p1.x, Render[i].p1.y, color);
    }
    OldLinestoRender = LinestoRender;
}

/***********************************************************************************************************************************/
// Sets the global vars for the 3d transform. Any points sent through "process" will be transformed using these figures.
// only needs to be called if Xan or Yan are changed.
void SetVars(void) {
    float Xan2, Yan2, Zan2;
    float s1, s2, s3, c1, c2, c3;

    Xan2 = Xan / fact; // convert degrees to radians.
    Yan2 = Yan / fact;

    // Zan is assumed to be zero

    s1 = sin(Yan2);
    s2 = sin(Xan2);

    c1 = cos(Yan2);
    c2 = cos(Xan2);

    xx = c1;
    xy = 0;
    xz = -s1;

    yx = (s1 * s2);
    yy = c2;
    yz = (c1 * s2);

    zx = (s1 * c2);
    zy = -s2;
    zz = (c1 * c2);
}

/***********************************************************************************************************************************/
// processes x1,y1,z1 and returns rx1,ry1 transformed by the variables set in SetVars()
// fairly heavy on floating point here.
// uses a bunch of global vars. Could be rewritten with a struct but not worth the effort.
void ProcessLine(struct Line2d* ret, struct Line3d vec) {
    float zvt1;
    int xv1, yv1, zv1;

    float zvt2;
    int xv2, yv2, zv2;

    int rx1, ry1;
    int rx2, ry2;

    int x1;
    int y1;
    int z1;

    int x2;
    int y2;
    int z2;

    int Ok;

    x1 = vec.p0.x;
    y1 = vec.p0.y;
    z1 = vec.p0.z;

    x2 = vec.p1.x;
    y2 = vec.p1.y;
    z2 = vec.p1.z;

    Ok = 0; // defaults to not OK

    xv1 = (x1 * xx) + (y1 * xy) + (z1 * xz);
    yv1 = (x1 * yx) + (y1 * yy) + (z1 * yz);
    zv1 = (x1 * zx) + (y1 * zy) + (z1 * zz);

    zvt1 = zv1 - Zoff;

    if (zvt1 < -5) {
        rx1 = 256 * (xv1 / zvt1) + Xoff;
        ry1 = 256 * (yv1 / zvt1) + Yoff;
        Ok = 1; // ok we are alright for point 1.
    }

    xv2 = (x2 * xx) + (y2 * xy) + (z2 * xz);
    yv2 = (x2 * yx) + (y2 * yy) + (z2 * yz);
    zv2 = (x2 * zx) + (y2 * zy) + (z2 * zz);

    zvt2 = zv2 - Zoff;

    if (zvt2 < -5) {
        rx2 = 256 * (xv2 / zvt2) + Xoff;
        ry2 = 256 * (yv2 / zvt2) + Yoff;
    } else {
        Ok = 0;
    }

    if (Ok == 1) {

        ret->p0.x = rx1;
        ret->p0.y = ry1;

        ret->p1.x = rx2;
        ret->p1.y = ry2;
    }
    // The ifs here are checks for out of bounds. needs a bit more code here to "safe" lines that will be way out of whack, so they dont get drawn and cause screen garbage.

}

/***********************************************************************************************************************************/
// 方块测试  line segments to draw a cube. basically p0 to p1. p1 to p2. p2 to p3 so on.
void cube(void) {
    // Front Face.

    Lines[0].p0.x = -50;
    Lines[0].p0.y = -50;
    Lines[0].p0.z = 50;
    Lines[0].p1.x = 50;
    Lines[0].p1.y = -50;
    Lines[0].p1.z = 50;

    Lines[1].p0.x = 50;
    Lines[1].p0.y = -50;
    Lines[1].p0.z = 50;
    Lines[1].p1.x = 50;
    Lines[1].p1.y = 50;
    Lines[1].p1.z = 50;

    Lines[2].p0.x = 50;
    Lines[2].p0.y = 50;
    Lines[2].p0.z = 50;
    Lines[2].p1.x = -50;
    Lines[2].p1.y = 50;
    Lines[2].p1.z = 50;

    Lines[3].p0.x = -50;
    Lines[3].p0.y = 50;
    Lines[3].p0.z = 50;
    Lines[3].p1.x = -50;
    Lines[3].p1.y = -50;
    Lines[3].p1.z = 50;


    //back face.

    Lines[4].p0.x = -50;
    Lines[4].p0.y = -50;
    Lines[4].p0.z = -50;
    Lines[4].p1.x = 50;
    Lines[4].p1.y = -50;
    Lines[4].p1.z = -50;

    Lines[5].p0.x = 50;
    Lines[5].p0.y = -50;
    Lines[5].p0.z = -50;
    Lines[5].p1.x = 50;
    Lines[5].p1.y = 50;
    Lines[5].p1.z = -50;

    Lines[6].p0.x = 50;
    Lines[6].p0.y = 50;
    Lines[6].p0.z = -50;
    Lines[6].p1.x = -50;
    Lines[6].p1.y = 50;
    Lines[6].p1.z = -50;

    Lines[7].p0.x = -50;
    Lines[7].p0.y = 50;
    Lines[7].p0.z = -50;
    Lines[7].p1.x = -50;
    Lines[7].p1.y = -50;
    Lines[7].p1.z = -50;


    // now the 4 edge lines.

    Lines[8].p0.x = -50;
    Lines[8].p0.y = -50;
    Lines[8].p0.z = 50;
    Lines[8].p1.x = -50;
    Lines[8].p1.y = -50;
    Lines[8].p1.z = -50;

    Lines[9].p0.x = 50;
    Lines[9].p0.y = -50;
    Lines[9].p0.z = 50;
    Lines[9].p1.x = 50;
    Lines[9].p1.y = -50;
    Lines[9].p1.z = -50;

    Lines[10].p0.x = -50;
    Lines[10].p0.y = 50;
    Lines[10].p0.z = 50;
    Lines[10].p1.x = -50;
    Lines[10].p1.y = 50;
    Lines[10].p1.z = -50;

    Lines[11].p0.x = 50;
    Lines[11].p0.y = 50;
    Lines[11].p0.z = 50;
    Lines[11].p1.x = 50;
    Lines[11].p1.y = 50;
    Lines[11].p1.z = -50;

    LinestoRender = 12;
    OldLinestoRender = LinestoRender;

}
